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Abstract 

We present structure preserving integrators for solving linear quadratic optimal control problems. This problem 
requires the numerical integration of matrix Riccati differential equations whose exact solution is a symmetric positive 
definite time-dependent matrix which controls the stability of the equation for the state. This property is not preserved, 
in general, by the numerical methods. We propose second order exponential methods based on the Magnus series ex- 
pansion which unconditionally preserve positivity for this problem and analyze higher order Magnus integrators. This 
method can also be used for the integration of nonlinear problems if they are previously linearized. The performance 
of the algorithms is illustrated with the stabilization of a quadrotor which is an unmanned aerial vehicle. 
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integrators 

2000 MSC: 49J15, 49N10, 34A26 



1. Introduction 

Nonlinear control problems have attracted the interest of researchers in different fields, e.g., the control of air- 
planes, helicopters, satellites, etc. Jsl [III [l3l during the last years. While the extensively studied linear quadratic 
optimal control (LQ) problems can be used for solving simplified models, most realistic problems are inherently non- 
linear. Furthermore, nonUnear control theory can improve the performance of the controller and enable tracking of 
aggressive trajectories |[l3t. 

Solving nonlinear optimal control problems requires the numerical integration of systems of coupled non-autono- 
mous and nonlinear equations with boundary conditions for which it is of great interest to have simple, fast, accurate 
and reliable numerical algorithms for real time integrations. 

It is usual to solve the nonlinear problems by linearization, which leads to problems that are solvable by lin- 
ear quadratic methods. In general, they require the integration of matrix Riccati differential equations (RDEs) itera- 
tively.The algebraic structure of the RDEs appearing in this problem implies that their solutions are symmetric positive 
definite matrices, a property that plays an important role for the quaUtative and quantitative solutions of both the con- 
trol and the state vector. 

Geometric numerical integrators are numerical algorithms which preserve most of the qualitative properties of the 
exact solution. However, the mentioned positivity of the solution in this problem is a qualitative property which is not 
unconditionally preserved by most methods, geometric integrators included. 

We show that some low order exponential integrators unconditionally preserve this property, and higher order 
methods preserve it under mild constraints on the time step. We refer to these methods as structure preserving inte- 
grators, and they will allow the use of relatively large time steps while showing a high performance for stiff problems 
or problems which strongly vary along the time evolution. 
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The aforementioned nonlinearities in the control problems can be dealt with in different ways. We consider three 
techniques to linearize the equations and the linear equations are then numerically solved using some exponential inte- 
grators which preserve the relevant properties of the solution. Since the nonlinear problems are solved by linearization, 
we first examine the linear problem in detail. 

The paper is organized as follows: The linear case is studied in Section |2] where we emphasize on the algebraic 
structure of the equations and the qualitative properties of the solutions. We next consider some exponential integrators 
and analyze the preservation of the qualitative properties of the solution by the proposed methods. In Section [3] it is 
shown how the full nonlinear problem can - after linearization - be treated as a particular case of the non-autonomous 
linear one. The work concludes with the application of the numerical algorithm to a particular example (control of a 
quadrotor) in Section 21 with which the accuracy of the exponential methods is demonstrated. Numerical results and 
conclusions are included. 

2. Linear quadratic (LQ) methods in optimal control problems 

Let us consider the general LQ optimal control problem 

min f '(x^{t)Q(t)X(t) + u^(t)R(t)u{t))dt (la) 

ueL^ Jo ' 

subject to X{t) ^ A{t)X{t) + B{t)u{t), X(Q) ^ Xq, (lb) 

where X{t) is the time-derivative of the state vector X{t) e M", u{t) e W" is the control, R{t) e M'"^'" is symmetric and 
positive definite, Q{t) e M."^" is symmetric positive semi-definite, A e R"^", B 6 R"^"" and denotes the transpose 
of a matrix M. 

Problems of the type O are frequent in many areas, such as game theory, quantum mechanics, economy, environ- 
ment problems, etc., see , or in engineering models ^ ch. 5]. 

The optimal control problem ^ is solved, assuming some controllability conditions, by the linear feedback con- 
troller iHili 

u(t) = -mx(t), (2) 

with the gain matrix 

K(t) = R-^\t)B\t)P(t), 

and P(t) verifying the matrix RDE 

m = -P(t)Mt) - A^{t)P(t) + P{t)Bm \t)B''{t)P{t) - Q{t), (3) 

with the final condition P{tf) - 0. The solution P{t) after backward time integration of Q is a symmetric and positive 
definite matrix when both Q{t) and R(t) are symmetric positive definite {I]] (similar results also apply for the weaker 
condition Q{t) positive semidefinite under general conditions on the matrices which make the system stabilizable and 
detectable). To compute the optimal control u{t), we solve for P{f), and plugging the control law into ( fTbl l yields a 
linear equation for the state vector 

X(t) = (A(f) - B(t)R-\t)B'^ (t)P{t))x{t), X(Q) = Xq, 

to be integrated forward in time, with which the control is readily computed. Notice that S{t) - B{t)R^\t)B^ (t) is a 
positive semi-definite symmetric matrix (positive definite if rank B = «) and P{t) is a positive definite matrix, hence 
its product is also a positive semi-definite matrix, and this is very important for the stability of the solution for the 
state vector and ultimately for the control. A numerical integrator which does not preserve the positivity of P{t) can 
become unstable when solving the state vector 

In this paper, exponential integrators, which belong to the class of Lie group methods (see ifrl figll and references 
therein), are proposed in order to solve the RDE Q. They are geometric integrators because they preserve some of 
the quaUtative properties of the exact solution. 
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2.1. Structure preserving integrators 

We are interested in numerical integrators which preserve the symmetry as well as the positivity of Pit). While 
symmetry is a property preserved by most methods, the preservation of positivity is a more challenging task. 

For our analysis, it is convenient to review some results from the numerical integration of differential equations. 
Given the ordinary differential equation (ODE) 



/(x,f), 



(4) 



the exact solution at time t - to + h can formally be written as a map that takes initial conditions to solutions, 
^h{xo) = x(fo + h). For sufficiently small h, it can also be interpreted as the exact solution at time f = fo + /i of an 
autonomous ODE 

-* = fh{x), Jc(fo) = xa, 

where /;, is the vector field associated to the Lie operator i log(<I>/,). 

In a similar way, a numerical integrator for solving the equation (|4| which is used with a time step /i, can be seen 
as the exact solution at time f = fo + /i of a perturbed problem (backward error analysis) 

■* = fhix), x(fo) = xo, 

and we say that the method is of order p if fh - fi, - 0{hP^^). The qualitative properties of the exact solution <!)/, are 
related to the algebraic structure of the vector field If the vector field fk associated with the numerical integrator 
shares the same algebraic structure, the numerical integrator will preserve these qualitative properties. 
Given the RDE 

P = PA{t) + A^(t)P - PS (t)P + Q(t), P(to) = 0, 

which is equivalent to (O with the sign of the time changed, i.e., integrated backward in time, and with Q(t),S{t) 
symmetric and positive definite matrices, then P{t), for t > to, is also a symmetric and positive definite matrix. Thus, 
a numerical integrator which can be considered as the exact solution of a perturbed matrix RDE 



P = PAi, 



■AlP- 



PShP + Qh 



P{to) = 0, 



where Qh,S h are symmetric positive definite matrices will preserve the symmetry and positivity of the exact solution. 
The same result applies if the numerical integrator is given by a composition of maps such that each one, separately, 
can be seen as the exact solution of a matrix RDE with the same structure. 

We will refer to these methods as positivity preserving integrators. If a method preserves positivity for all h > 0, 
we say it is unconditionally positivity preserx'ing and, if there exists h* > such that this property is preserved for 
Q < h < h* , we will refer to it as conditionally positive preserving. 

In general, standard methods do not preserve positivity. We show, however, that some second order exponen- 
tial integrators preserve positivity unconditionally and higher order ones are conditionally positivity preserving for 
a relatively large range of values of h* which depends on the smoothness in the time dependence of the matrices 
A(f), S (f), Q{t). At this stage, it is convenient to rewrite the RDE (O as a linear differential equation 
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-Qit) 




Vit) 
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Wit) 
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Vitf) 
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where Pf -0, S it) = Bit)R ' it)B^it) and the solution Pit) of problem (O, to be integrated backward in time, is given 
by 

Pit) = Vit)Wit)-\ Pit), Vit), Wit) e M"^", 

in the region where Wit) is invertible (see, for instance, 101 or lEoll . and references therein). If Rit) and Qit) are 
positive definite matrices, this problem always has a solution. 

It is then clear that if a numerical integrator for the equation Q can be regarded as the exact solution of an 
autonomous perturbed linear equation 
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where Qh and Si, are symmetric and positive definite matrices, then the numerical solution is symmetric and positive 
definite. 

In general, high order standard methods like Runge-Kutta methods do not preserve positivity. Explicit methods 
applied to the hnear problem do not preserve positivity unconditionally, but to show this result for implicit methods 
requires a more detailed analysis and it is stated in the following theorem. 

Theorem 2.1. The second order implicit midpoint and trapezoidal Runge-Kutta methods do not preserve positivity 
unconditionally for the solution of the RDE Q. 

Proof 2.2. It suffices to prove it for the scalar non- autonomous problem 

p - -q- 2a(t) p + sp^, p(tf) = (6) 

with q, s > and a : [0, f/] M. Let a{t) — Ofor t e [tf — h, tf] and ait) — a < Q for t G [0, tf — 3h/2]. Then, for the 
implicit midpoint rule, two iterations backwards in time starting with po — yield a negative value P2 < if\a\ > 2/h. 
For the trapezoidal rule, three iterations are necessary to reach negative values, with a sufficient condition for pi < 
being 

3 , / 1 6 \ ,2 

a< h/ifl — H —\ A h < —-. 

h ^\ 4 -4 + h^ql ^Jq 

We remark, that, given a < Q, the method produces negative values p^ for a range of time-steps, i.e., for larger 
time-steps h, it is less prone to negativity. Furthermore, not only can the methods produce negative values pi,, for 
certain parameter ranges they also attain complex valued results. 

To better illustrate the results, we consider the problem (|6]l for 

q^s^l, a{t)^- — , f/ = 10, (7) 

^ 1 + exp(-4(f - f//2)) ^ 

with h = 1/2 for the equation of p(t) and /i = 1 for the equation of x{t), 

i = (a (f) - sp (0) X, x(0) = 0. 

We integrated the problem using the second order implicit trapezoidal and midpoint methods as well as the first order 
implicit Euler method. The results are shown in Figure [1] where we appreciate that the first order implicit Euler 
method is superior (the results obtained with the second order Magnus integrator to be presented in the next section is 
also included). The poor performance and non-positivity of the higher order standard implicit methods is manifest. 

If we are interested in high order numerical integrators, different classes of methods have to be explored. We 
consider a particular class of exponential integrators referred to as Magnus integrators (see (01 and references therein). 
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2.2. Magnus integrators 

Given the general linear equation 



y'^M(t)y, 



y{to) = yo , 



(8) 



with y e W, and if we denote the fundamental solution by <^(t, to) € W^p, such that y{t) - 0(f, fo)3'(fo), the Magnus 
expansion gives us the formal solution (under certain convergence conditions, see and references therein) as 

O(f,fo) = exp(Q(f,fo)), 

where Q(f, fo) = 2;tLi f^n(f> ^o) and each 0„(f, fo) is an element of the Lie algebra generated by «-dimensional integrals 
involving « - 1 nested commutators of M(t) at different instants of time. The first two terms are given by 



fii(f,fo) 



/ 

J to 



M(s) ds. 



^2it, fo) 



- f dti f 

2 Jto Jlo 



[M(ti),M(t2)] dt2 



where [A,B] ^ AB - BA. 

In the region of convergence of the Magnus expansion, the exact solution at time t = to + h is equivalent to the 
exact solution of the autonomous linear equation 



1 



y 



-Q(fo + /i,/o)y, 



y(to) = yo ■ 



It is well known that the set of matrices 



A 
C 



B 



(9) 



with A,B,C € R"^" and B - B^, C - form the algebra of symplectic matrices. This algebraic property is 
preserved by the commutators and then any truncated Magnus expansion preserves symplecticity for this problem. 
However, the additional property about the positivity (or negativity) of the skew diagonal matrices B, C is not always 
guaranteed when the series is truncated. We analyze lower order methods and show that it is possible to build second 
order Magnus integrators which unconditionally preserve positivity. 

The first term in the expansion applied to (|5]l does not contain commutators and is given by 



Qi(f,fo) = 



Jto 



A{sf ds 



S(s) ds 



-/' 

Jlo 

f 

Jto 



Q(s)ds 



A{s) ds 



(10) 



Then, if we truncate the series after the first term and approximate the integrals for a time interval f e [fo, fo + h] 
using a quadrature rule of second or higher order, we obtain a second order method. 

It is well known that, if Q{t) is a symmetric positive definite matrix for f 6 [fo, fo + h], then Qi, = f'"^'' Q(s) ds is 
also symmetric positive definite. Suppose now that the integral is approximated using a quadrature rule 



* nlo+h 

Qk=hY^biQ{to + Cih)^ \ Q(s)ds 

1 = 1 ^'o 



with c, e [0, 1], ! = 1, . . . , A:. If 2, > , we have: 

a) If bi > 0, i - I, . . . ,k, then Qi, is a symmetric positive definite matrix. 

b) If 3 bj < 0, for some value of j and \\Q(tm) - Q(tn)\\ < C\t„ - f„|, Vf„,, f„ 6 [fo, fo + h], then 3 h* > such that g;, 

is a symmetric positive definite matrix for Q < h < h*, and h* depends on C. 
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The same results also apply to 5/,. 

A second order method which preserves positivity is constructed by taking the first term in the Magnus expansion 
( fTOl ) and approximating the integrals by a second or higher order rule with the constraint that all /?, > 0. The most 
natural choices are the midpoint rule 

= exp iliM(t + h/2)) = (t)(f + t) + 0(h\ 

or the trapezoidal rule 

'Pf' = exp 1^ [M(r + h) + M{t)]j = (D(f + h, t) + O(h^). (11) 

The latter of which is found more efficient since less evaluations of the functions in the algorithm are necessary as 
they can be reused in the computation of X(t). If we consider the RDE (O that corresponds to ([8]) with the data (|5]i 
and choose an equidistant time grid t„ - to + nh, Q < n < N , with constant time step h - (tf - t())/N and taking into 
account that this equation has to be solved backward in time, we obtain 



" v„ 




w„ 


= exp| 



-^[M(f„) + M(f„+i)] 



By construction, P„ is a symmetric positive definite matrix. In addition, it is also a time symmetric second order 
approximation to In this way, the matrix functions A(f„), B{t„), Q{tn), R{tn) are computed at the same mesh 

points as the approximations P/, of P{t) and, as we will see, they can be reused for the forward time integration of the 
state vector. 

Let us consider the equation for the state vector, to be integrated forward in time, which takes the form 

X = {A(t)-S(t)P,)X, 

where we denote by P/, the numerical approximations to P{t) computed on the mesh and Pi, „ ^ P{t„). Notice that 
at the instant t = tf - h, we have that Ph,N-i - P{tf - h) + O(h^) (local error) but at f = fo, after steps, we have 
Ph,o - Pito) + 0{h^) (global error). This accuracy suffices to get a second order approximation for the numerical 
approximation to the state vector 

If we use the same Magnus expansion for the numerical integration of the state vector with the trapezoidal rule, 
we have the algorithm 

X„+i = exp 1^ [D„+i + D„]j Xn , D,„ = A„, - S,„Ph,m, m^n,n+\, 

where A„, = A(f„,), 5„, = S(t„,). 

Finally, the controls which allow us to reach the final state in a nearly optimal way are computed via 

M„ = -R-\t„)B^(tn)P„X„. 

Higher order Magnus integrators. Truncating the Magnus expansion at higher powers of h usually requires the 
computation of matrix commutators. If we include, for example, the second term Q2 in the exponent, we obtain 
*P/, s exp (Q.I + Q.2), which agrees with the exact solution up to order four, i.e., - ^{t + h, t) + 0{li^). The sum 
Qi + Q2 belongs to the algebra of symplectic matrices, as given in (|9]l, where the off-diagonal matrices B, C take an 
involved form. We will show that positivity is conditionally preserved, however, unconditional preservation as for 
exp(f2i) cannot be achieved. 

For simplicity in the analysis, we consider commutator-free Magnus integrators (see ||3.Etl and references therein). 
With the abbreviations 

M^"' = J M(s) ds, M^'^ = - j - \t„ + -jj M(s) ds, 

the following commutator-free composition gives an approximation to fourth-order 

= exp + 2M*'>j exp - 2M<" j = (D(fo + h, fo) + O(h^). 



Using the fourth-order Gaussian quadrature rule to approximate the integrals yields 

= exp (h(j3Mi + aM2)) exp (h{aMi + /3M2)) , 

where M,- = M{t„ + cji), / = 1,2, ci = i - C2 = ^ + ff = i - = -0.038 . . . < 0, ^ = 5 + "T- This 
composition will not preserve positivity unconditionally when applied to solve the RDE because a < 0. However, 
since a + yS = j the positivity will be conditionally preserved. 

If we approximate the integrals using the Simpson rule, we have 

" exp j^(-Mi + 4M2 + 3M3)j exp (^(3Mi + AM2 - M3)j , 

where Mi = M{t„), M2 = M{t„ + li/2), M3 s M{t„ + h). As previously, one of the coefficients is negative and positivity 
is not unconditionally preserved when the method is applied to the RDE. 

Recall that the full problem requires the solution of two differential equations; suppose we want to (backward) 
integrate the matrix RDE with one of the fourth-order commutator-free methods and then use the same method for 
the (forward) integration of the state vector, we need to use a time step twice as large for the forward integration 
(preferably with the Simpson rule, since no interpolation is necessary). 

The main goal of this paper is to present a simple, fast, accurate and reliable numerical scheme for nonlinear 
problems. As we will see, nonlinear problems are linearized, and the resulting linear equations are solved iteratively. 
The solution of each iteration is plugged into the following iteration, and this requires to use a fixed mesh for all 
methods. For this reason, the second order Magnus integrator is the optimal candidate among the previous and is used 
in the numerical examples in section |4] 

3. The nonlinear control problem 

Many problems in engineering can be stated as optimal control problems of the form 

min {x^{t)Q{t,X{t))X{t) + u^it)Rit,X{t))u{t)\dt (12a) 
"ei-- Jo ^ ' 

subject to X{t) = Ja (t, X (f)) + /b (t, X{t),u (f)) , X{<d) = Xq. (12b) 

This nonlinear optimal control problem is considerably more involved than its linear counterpart. It is then usual 
to solve the nonlinear problem by linearization, and this can be done in different ways. In the following, under the 
assumption that fs depends linearly on u, we present three of them and compare their performances when the linear 
equations are solved using exponential integrators. 

Quasilinearization. For /^(f, 0) = and /^(f, X,u) i^O for all f, X in the appropriate domains, the state equation ( I12bl l 
can be written in a non-unique way as 

Xit) = Ait,X)X{t) + B(t,X)u(t), X{Q) = Xq. (13) 

The formulation ( fT3T l is the basic ingredient for the State Dependent Riccati Equation (SDRE) control technique 
in. [l6ll . Its formal similarity to the linear problem ^ motivates the imitation of the optimal LQ controller by defining 

u{t) = -R-\t)B'^{t,X{t))P{t,X)Xit) (14a) 
where P{t, X) solves the now state-dependent algebraic Riccati equation 

= -PA{t,X)-A{t,XfP + PB{t,X)R(t,Xy^B(t,XfP-Q{t,X). (14b) 

One has to choose the unique positive definite solution of the algebraic Riccati equation and, combining ( I14al i with 
([13] ). the closed-loop nonlinear dynamics are given by 

X = [A{t,X)- Bit,X)R{t,Xy^B{t,XfP(t,X))x, X(Q) = Xq. (14c) 
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The usual approach is to start from X{0) - Xq, and then to advance step by step in time by first computing P from 
(I14bl i at each step and then applying the Forward Euler method on (I14cl i. The application of higher order methods, 
such as Runge-Kutta schemes, requires to solve implicit systems with ( I14bb and can thus be costly. In addition, if one 
is interested in aggressive trajectories, the algebraic equation ( I14bb can considerably differ from the solution of the 
corresponding Riccati differential equation, which affects the solution of the state vector, X, and ultimately the control 
in ( I14ab . 



Waveform relaxation. Alternatively, we can linearize (I14cb . by iterating 

[A(t,X") - B(t,X")R{t,X")-^B(t,X"fP(t,X"))x" 



dt 



(15) 



We start with a guess solution X^{t) and iteratively obtain a sequence of solutions, X\t), X^{t), . . .,X"{t). Again, the 
iteration stops once consecutive solutions differ by less than a given tolerance. Here, P{t,X"{t)) at each iteration is 
obtained from 

p = -PA"{t) - A"{tfP + PB"(t)R''(t)-^B"{tfP - Q"{t), P{tf) = 0, (16) 
with A"(f) = A{t,X"{t)), B"{t) = B{t,X"{t)), etc. 



This procedure is similar to what is known as waveform relaxation 02411 . however, the backward integration for 
P limits the parallelizability in this application. This approach corresponds to freezing the nonlinear parts in ( fT3] ) at 
the previous state and then applying the optimal control law (|2]i. It is worth noting that this technique can handle 
inhomogeneities by slightly adapting the control law, at the cost of solving an inhomogeneous linear system, see 
below. The algorithms are illustrated in Table [1] 



Al: waveform relaxation 


A2: linearization 


n := 0; guess : X°{t), u^(t) 


n;=0; guess : X°{t),if{t) 


do 


do 


n := n + I 


n := ;j + 1 


compute : A"-^{t), B"-\t) 


compute : A"-\t), B"-'{t), C"-\t) 


solve (f^ ^ 0) : eq. lfT6t forP" ' 


solve (?y 0) : eq. Q/orP"-' 


solve (0 ^ tf) : eq. ifBIl for X" 


solve (ry ^ 0) : eq. ^^for V" 


while \X" - > tolerance 


solve (0 ^ tf) : eq. ^forX" 


Check for feasibility ofX" 


while \X" - > tolerance 




Check for feasibility ofX" 



Table 1; Algorithm (Al) for the waveform relaxation and algorithm (A2) for the Taylor-type linearization. 



Taylor-type linearization. Similarly to ll22ll . we can Taylor-expand the vector field in ( I12bl ) around an approximate 
solution X"(t) and use optimal LQ controls for the approximated equation. The iteration step reads then 

X«+i(f) = A"(t)X"^\t) + B'\t)u"^\t) + C\t), (17) 

where 

A%t) = DxfA (t, X"it)) + DxfB (t, X"(t), u"(t)) , 
B"(t) ^ DufB{t,X'\U"), 

C"(t) = fA(t, X") + feit, X", u") - (A"(f) ■ X" + B"(t) ■ u") , 

and Dx denotes the derivative with respect to X, etc. One starts with an initial guess, X'^(t) and the iteration stops once 
consecutive iterations differ by less than a given tolerance. 

The inhomogeneity C" can be treated as a disturbance input and compensated by the controller ifioll . The optimal 
control then becomes 

M"+i(f) = -R"(t)-^B"(tf [p'\t)X"^\t) + V"{t)) 
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where P"(f) satisfies (O with replacements A — > A" and B B", etc. and V"{t) is given by 

V ^[PBR-^B^ - A^)V - PC, V{tf)^0 
at each iteration. The Hnearization procedure is summarized in Table [1] 



(18) 



NOTE: We can solve non-homogeneous equations with Magnus integrators as follows. Given the non-homogeneous 
equation 

y ^M(t)y +C(t), y(to)^yo; 
it can be formulated as a homogeneous one in the following way 



d 


y 




M(r) 


C{t) 




y 


dt 


1 











1 



, WO), 1]^ = [yo, 1]' 



whereof = [0,...,0] e K«. 



4. Modeling the control of a quadrotor UAV 

The optimal control of Unmanned Aireal Vehicles (UAV) has attracted great attention in recent years ifTH [Till . 
Helicopters are classified as Vertical Take Off Landing (VTOL) aircraft and are among the most complex flying 
objects because their flight dynamics are nonlinear and their variables are strongly coupled. 

In this section, we address the optimal control of a quadrotor, i.e., a vehicle with four propellers, whose rotational 
speeds are independent, placed around a main body S |^ [l2 , 14 , 1/7 1 . Linear techniques to control the system have 
been frequently used. The controllers are designed based on a simplified description of the system behavior (linearized 
models). While this is satisfactory at hover and low velocities, it does not predict correctly the system behavior during 
fast maneuvers (most controllers are specifically designed for low velocities) and in order to improve the performance, 
the nonlinear nature of the quadrotor has to be taken into account |[l5 , 23 ] . In addition, problems can have time-varying 
parameters |25| or require time-dependent state references iITtIi . 

Under realistic conditions, real time calculations are necessary since the optimal control will have to adjust to 
environmental changes, that are not accounted for in the model, and hence more efficient and elaborated algorithms 
have to be designed. 

LQ optimal controllers are widely used, in particular for the control of small aircrafts fs, 23], where they have 
shown to produce better results than other standard methods, like proportional integral derivative methods (PID) |@]. 
The techniques presented here, however, are valid for the general optimal LQ control problem ([TJ. 

For the illustration of our methods, we consider a VTOL quadrotor, based on the model presented in lfl4il23ll (and 
references therein). Figure |2] describes the configuration of the system, where (p, 6 and iff denote the rolling, pitching 
and yawing angles, respectively. We assume some standard general conditions on the symmetric and rigid structure 




Figure 2: Quadrotor schematic 
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of the flying robot: the center of mass is in the center of the planar quadrotor and the propellers are rigid. 

We remark that inhomogeneities //i(f, 0) = b{t), e.g., from gravitational forces, can be treated as disturbances, by 
adding new state variables or by taking advantage of non-vanishing states, e.g., the altitude of the UAV when hover is 



searched 1115 1. 

An analysis of the dynamics of the quadrotor shows that the control of the attitude can be separated from the 
translation of the UAV 12311 and we focus our attention on the stabilization of the attitude, neglecting the gyroscopic 
effect. The state vector is given by 

x(t) = (0(0, m m, m, m, 4,(1)) 



T 



and the input vector m e is formed by linear combinations of the thrust of each propeller. 

The system designer can choose the weight matrices to tune the behavior of the control according to the require- 
ments, R{t) is used to suppress certain movements and Q{t) limits the use of the control inputs. Usually, these matrices 
are chosen constant, positive definite and often even diagonal, see p. 67], lfl4[[l7ll . For the numerical experiments, 
we have implemented the problem (fTSl i with the following values taken from (|8l |231] 

fll,2 = fl3,4 = a5,6 = 1, a2A-^aiIitp, fl2,6 = - <3'l)/l6' 

04,2 - y^a2h<P, ciAfi - M} - axjhifi, cib.i - Aa^hQ, (19) 

fl6,4 = ^(1 - ax)h4>, bo.l - LI I,,, ^4,2 = Lily, ^76,3 = 1//; 

where c,- reflects the non-uniqueness in the SDRE formulation, A denotes the inflow ratio, L is the length of the arms 
connecting the propellers with the center and the relative moments of inertia are I\ - (/, - L)IIx, h - (L " I\)IIy, 
h - {I.K - Iy)IL. Here, niij denotes the element located at ;-th row and j-th column of the matrix M. Other entries of 
A(t) € M^^'' and B(t) € M^^^ not indicated in ^ are null elements. 

The numerical values are extracted from ||8[| and are given in the SI units 

= 0.0075, = 0.0075, 0.0130, L = 0.23, A ^ I, a, = 1. 

The weight matrices are fixed at 

Q = 0.01 ■ diag{ 1,0.1, 1,0.1, 1,0.1) e W"'^ R = diag{l,0.1, 1) e M?''\ 

We set the time frame to fy - 10 seconds, with a stepsize of h - 0.125i and initial state 

Xo = (70°, 10, 70°, 20, -130°,-l/, 

that corresponds to a disadvantageous orientation and high rotational velocities that are sought to be stabilized at 
6 R'' at the final time tf. 

We have implemented a variety of methods to test against the Magnus integrators presented in section 12.21 As 
initial condition, we have taken X^\t) = (1 - tltf)Xo and the iteration was stopped when \\X" - X""'||2 < lO"-'. We 
use the explicit and implicit Euler methods as well as the second order Magnus integrator. Some experimental results 
are given in Table |2] where we can see that the Magnus based method (fTTT i. approximates the optimal control best. 
However, we have to remark that the SDRE method is for the given parameters about a factor ten faster, due to 
necessary iterations for the other schemes. 

Figure [3] shows the controls obtained for the schemes S2, W3, T3 and Figure |4] shows the motion of the quadrotor 
angles subject to the controls. We can appreciate how the Magnus methods maximize the use of the controls to reach 
an overall minimum of the cost functional. 

From the numerical experiments we conclude that Lie group methods such as Magnus integrators which preserve 
the positivity of the solution of the matrix RDE are very useful tools for solving optimal control problems of UAV. 



5. Conclusions 

We have presented structure preserving integrators based on the Magnus expansion for solving linear quadratic 
optimal control problems. The schemes considered require the numerical integration of matrix RDEs whose solu- 
tions, for this class of problems, are symmetric and positive definite matrices. The preservation of this property is 
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Type 


X(t) 


P{t) 


V(r) 


Cost 


It. 


SI) 


SDRE 


Euler 


are 


N/A 


0.1114 




S2) 




Impl. Euler (IE) 


are 


N/A 


0.1021 








Optimal 


=> 




0.0977 




Wl) 


WAVE 


Euler 


Euler 


N/A 


0.1071 


3 


W2) 




IE 


IE 


N/A 


0.1036 


3 


W3) 




Magnus (fTTT) 


Magnus 


N/A 


0.0926 


3 






Optimal 


=> 


N/A 


0.0888 




Tl) 


TAYLOR 


Euler 


Euler 


Euler 


N/A 


Inf 


T2) 




IE 


IE 


IE 


0.0789 


12 


T3) 




Magnus 


Magnus 


Magnus 


0.0707 


12 






Optimal 


=> 




0.0707 





Table 2: Comparison of numerical methods. Type indicates the linearization procedure given by section[3]and It. denotes the number of iterations 
necessary until convergence. The cost is a discrete approximation of the integral in )12a) . 
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-1 





-1 1 
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\ 






\ 

\ 








^Jf^^^ \ ■» ■» 

_l 1 





0.5 



1.5 



Figure 3: Evolution of the control vector. The left column shows the control that has been least penalized U2. All curves are given 
for all methods S2 (line), W3 (diamond) and T3 (cross). 



very important to obtain reliable and efficient numerical integrators. While geometric integrators preserve most of 
the qualitative properties of the exact solution, the preservation of positivity for the matrix RDE is, in general, not 
guaranteed. We have shown that some symmetric second order exponential integrators (Magnus integrators) preserve 
this property unconditionally and, in addition, are very appropriate to build simple and efficient numerical algorithms 
for solving nonlinear problems by linearization. The performance of the methods is illustrated with an application to 
stabilize a quadrotor UAV. The results shown for a quadrotor easily extend to other helicopters. 

For more involved trajectories, the structure of the equations will play a more important role and the methods 
presented in this work could be very useful in those cases. Additionally, in more difficult settings, e.g., in the case 
of trajectory following or obstacle avoidance, stronger time dependencies of the parameters are expected, making 
standard methods more susceptible to instabilities, and thus, the advantages of the exponential methods are expected 
to be amplified. This tendency highlights these applications as interesting for further investigation. 
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Figure 4: Evolution of the orientation of the quadrotor (top row) and angular velocities (bottom). The left column shows the 
coordinates 9(t) and 9(1), whereas the remaining coordinates (fi(t), ^{t) and il/(t), <p{t) are depicted in the right column. All curves 
are given for all methods S2 (line), W3 (diamond) and T3 (cross). 



References 

[1] H. Abou-Kandil, G. Freiling, V. lonescu & G. Jank, (2003) Matrix Riccati equations in control and systems theory. Virkauser Verlag, Basel. 

[2] B. D. O. Anderson & J. B. Moore, (2007) Optimal control. Linear quadratic methods. Dover Publications. New York. 

[3] C. Balas, (2007) Modelling and linear control of a quadrotor. MSc Thesis, School of Engineering, Cranfield University. England. 

[4] S. Blanes, F. Casas, J. A. Oteo & J. Ros, (2009) The Magnus expansion and some of its applications. Physics Reports, 470, pp. 151-238. 

[5] S. Blanes & P. C. Moan, (2006) Fourth- and sixth-order commutator-free Magnus integrators for linear and non-lineai' dynamical systems. 

Appl. Num. Math., 56, pp. 1519-1537. 
[6] S. Blanes & E. Ponsoda (2012) Magnus integrators for solving linear-quadratic differential games. /. Comp. Appl. Math., 236, pp. 3394-3408. 
[7] S. Blanes & E. Ponsoda (2012) Time-averaging and exponential integrators for non-homogeneous linear IVPs and EVPs. Appl. Num. Math, 

62, pp. 875-894. 

[8] S. Bouabdallah, (2006) Design and control of quadrotors with application to autonomous flying. Ph.D. dissertation, EPFL. 
[9] S. Bouabdallah, A. Noth & R. Siegwart, (2004) PID vs LQ control techniques applied to an indoor micro quadrotor Proc. of the lEEE/RSJ 
Int. Conf. on Intelligent Robots and Systems (IROS 2004), 3, pp. 2451-2456. 
[10] A. Biyson Jr & Y. C. Ho, (1975) Applied Optimal Control, Halsted. 

[11] A. Budiyono & S. S. Wibowo, (2007) Optimal tracking controller design for a small scale helicopter. / Bionic Eng., 4, pp. 271-280. 
[12] P. Castillo, A. Dzul & R. Lozano, (2004) Real-time stabilization and tracking of four rotor mini-rotorcraft. IEEE Trans. Control Syst Tech., 
12, pp. 510-516. 

[13] P. Castillo, R. Lozano & A. Dzul, (2005) Stabilization of a mini rotorcraft with four rotors. Experimental implementation of linear and 

nonlinear control laws. IEEE Control Systems Magazine, pp. 45—45, Dec. 2005. 
[14] P. Castillo, R. Lozano & A. E. Dzul, (2005) Modelling and control of mini-flying machines. Advances in Industrial Control Series. Springer. 

London. England. 

[15] T. Qimen, (2008) State-dependent Riccati equation (SDRE) control: A survey. Proc. of the 1 7th IFAC World Congress(IFAC'08) Seoul, 
Korea, pp. 3761-3775. 

[16] J. Cloutier, (1997) State-Dependent Riccati Equation Techniques: An Overview. Proc. of the American Control Conference, Albuquerque, 
New Mexico, 2, pp.932-936. 



12 



[17] I. D. Cowling, J. F. Whidborne & A. K. Cooke, (2006) Optimal trajectory planning and LQR control for a quadrotor UAV. Proc. UKACC Int. 

Conf. Control 2006 (ICC 2006), Glasgow, UK. 
[18] J. Engwerda (2005) LQ dynamic optimization and differential games. John Wiley and sons. 
[19] A. Iserles, H. Z. Munthe-Kaas, S. P. N0rsett & A. Zanna, (2000) Lie group methods. Acta Numerica, 9, 215-365. 

[20] L. Jodar & E. Ponsoda, (1995) Non-autonomous Riccati-type matrix differential equations: existence interval, construction of continuous 

numerical solutions and error bounds. IMA J. Num. Anal., 15, 61-74. 
[21] D. Kirk, (2004) Optimal control theory, an Introduction. Dover Publ., Mineola, New York. 

[22] E. Ponsoda, S. Blanes & P. Bader, (2011) New efficient numerical methods to describe the heat transfer in a solid medium. Math. Comput. 
Mod.. 54, pp. 1858-1862. 

[23] H. Voos, (2006) Nonhnear state-dependent Riccati equation control of a quadrotor UAV. Proc. Int. Conf. Control Appl. Munich, Germany, 
pp. 2547-2552. 

[24] J. White, F. Odeh, A.S. Vincentelli & A.Ruehli, (1985) Waveform relaxation: theory and practice. Trans. Soc. Comput Simulation, 2, pp. 
95-133. 

[25] R. Zhang, Q. Quan & K.-Y. Cai, (201 1 ) Attitude control of a quadrotor aircraft subject to a class of time-varying disturbance. lET Control 
Theory Appl, 5, pp. 1 140-1 146. 



13 



